#ifndef OMPL_BASE_STATE_TRANSFORMER_H
#define OMPL_BASE_STATE_TRANSFORMER_H

#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/SE3StateSpace.h>
#include <ompl/base/spaces/SE2StateSpace.h>
#include <ompl/base/spaces/SO3StateSpace.h>
#include <ompl/base/spaces/SO2StateSpace.h>
#include <ompl/util/ClassForward.h>


namespace ompl
{

namespace base
{

ClassForward(StateToVectorTransformer);

class StateToVectorTransformer
{
public:
  StateToVectorTransformer(const SpaceInformationPtr& si);

  ~StateToVectorTransformer();

  unsigned int dim() const
  {
    return dim_;
  }

  void transform(const State* state, StateSpacePtr space, unsigned int& offset) const;

  float* transform(const State* state) const
  {
    unsigned int offset = 0;
    transform(state, space_, offset);
    return cache_;
  }

  float* getLowerBound() const
  {
    return lower_bound_;
  }

  float* getUpperBound() const
  {
    return upper_bound_;
  }

  float* getWeights() const
  {
    return weights_;
  }

  bool* getTypes() const
  {
    return types_;
  }

  unsigned int nonEuclideanDim() const
  {
    unsigned int tmp = 0;
    for(unsigned int i = 0; i <= dim_; ++i)
    {
      if(types_[i] == false)
        tmp++;
    }

    return tmp;
  }

private:
  StateSpacePtr space_;

  unsigned int dim_;

  mutable float* cache_;

  float* upper_bound_;

  float* lower_bound_;

  float* weights_;

  bool* types_; // true: euclidean; false: non-euclidean


  void quaternionToEulerAngles(double x, double y, double z, double w, double& a, double& b, double& c) const
  {
    a = atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y));
    b = asin(2 * (w * y - z * x));
    c = atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z));
  }

  void computeDim(StateSpacePtr space, unsigned int& dim);

  void getData(StateSpacePtr space, unsigned int& offset);

  void getWeights(StateSpacePtr space, unsigned int& offset, float weight);
};

}

}


#endif
